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Abstract 


This  paper  presents  a  kinetic-model  based  algorithm  for  estimating  some  unstabilized  compo¬ 
nents  in  vehicular  motion.  In  addition  to  smooth  movement,  there  are  unstabUized  components 
such  as  bounce,  pitch  and  roll  in  vehicular  motion.  To  reliably  accomplish  other  tasks  like  track¬ 
ing  and  obstacle  avoidance  using  visual  inputs,  it  is  essential  to  consider  these  disturbances.  A 
two-wheel  vehicle  model  available  in  the  literature  is  used  for  this  purpose.  It  takes  into  account 
the  bouncing  and  pitching  components.  The  dynamics  of  these  unstabilized  components  are  for¬ 
mulated  using  standard  equations  of  motion.  Assuming  that  depth  information  is  known  for  some 
landmarks  in  the  scene  (e.g.,  obtained  from  a  laser  range  finder)  and  additional  information  from 
inertial  sensors  such  as  accelerometers  is  available,  a  feature-based  approach  is  proposed  to  esti¬ 
mate  the  unstabilized  components.  Simulation  results  for  both  deterministic  and  stochastic  terrain 
profiles  are  presented.  The  robustness  of  the  filter  with  respect  to  various  parameter  mismatches 
is  also  addressed. 


The  support  of  the  Advanced  Research  Projects  Agency  (ARPA  Order  No.  A422)  and  the  Army  Research  Office 
under  Grant  DAAH-0493G0419  is  gratefully  acknowledged,  as  is  the  help  of  Sandy  German  in  preparing  this  paper. 


1  Introduction 


There  has  been  growing  interest  among  computer  vision  researchers  in  solving  the  problem  of  au¬ 
tonomous  vehicle  navigation.  For  many  navigation-related  tasks  such  as  feature  tracking,  moving 
object  detection,  obstacle  avoidance,  etc.,  knowledge  of  the  vehicle’s  pose  and  motion  is  a  prereq¬ 
uisite  for  success.  Although  the  Inertial  Navigation  System  (INS)  on  board  the  vehicle  can  provide 
accurate  attitude  and  motion  information  over  short  periods,  there  exist  some  problems  over  long 
periods  due  to  sensor  drifts.  An  independent  estimate  of  the  vehicle’s  motion  can  be  combined 
with  the  INS  data  to  provide  more  reliable  information. 

In  recent  years,  the  wealth  of  information  contained  in  long  sequences  of  images  has  attracted 
the  attention  of  computer  vision  researchers.  Due  to  lack  of  knowledge  of  the  forces  and  torques 
that  result  in  movements  of  the  camera,  most  kinematic-model-based  motion  estimation  algorithms 
.assume  a  smooth  trajectory  over  time  in  order  to  exploit  temporal  information  [1, 11-14].  However, 
for  a  vehicle  moving  in  an  outdoor  environment,  the  onboard  camera  undergoes  non-smooth  motion. 
The  performance  of  algorithms  which  do  not  take  into  account  unstabilized  components  in  the 
motion  may  degrade,  as  noted  in  [4]. 

In  order  to  describe  jerky  movements  of  a  vehicle,  a  two-wheel  vehicle  model  which  can  be  found 
in  the  literature  on  optimal  design  of  suspension  systems  is  utilized  [2].  This  model  accounts  for 
two  unstabilized  components  of  the  motion:  bounce  and  pitch.  In  addition  to  compensation  for 
the  smooth  motion  assumption,  the  explicit  separation  of  unstabilized  and  stabilized  components 
provides  useful  information  for  stabilizing  all  the  sensors  on  the  vehicle. 

In  this  paper,  to  estimate  these  components,  the  equations  of  motion  are  first  derived  from 
the  Lagrangian  point  of  view.  Subsequently,  assuming  that  a  camera  is  rigidly  attached  to  the 
vehicle  with  known  orientation  and  the  3-D  coordinates  of  some  landmarks  are  available  (these 


can  be  obtained,  for  example,  by  a  vehicle-mounted  laser  range  finder),  the  relationships  between 


the  vehicular  motion  and  image  plane  displacements  of  these  landmarks  are  found.  A  recursive 
algorithirl  is  then  formulated,  and  an  Iterated  Extended  Kalman  Filter  (lEKF)  [7]  is  used  to  estimate 
both  unstabilized  components  of  the  vehicular  motion.  In  addition  to  the  visual  information, 
inertial  sensors  such  as  accelerometers  are  also  incorporated  in  our  work.  Since  the  model  requires 


various  internal  parameters  such  as  the  moment  of  inertia  and  the  position  of  the  center  of  gravity, 
the  robustness  of  the  filter  with  respect  to  parameter  mismatches  is  also  studied  analytically  and  o 
numerically.  , 


Avail' 


The  organization  of  this  paper  is  as  follows.  Section  2  gives  detailed  descriptions  of  the  two- wheel 
vehicle  model  and  the  physical  laws  describing  the  dynamics  of  both  unstabihzed  components.  The 
recursive  formulation  of  the  algorithm  is  given  in  Section  3.  Section  4  presents  simulation  results. 
A  sensitivity  analysis  is  given  in  Section  5,  and  conclusions  are  presented  in  Section  6. 

2  Vehicle  Model 

In  order  to  account  for  unstabilized  components  such  as  bounce  and  pitch  in  vehicular  motion, 
the  two- wheel  vehicle  model  [2]  shown  in  Figure  1  is  used.  Both  front  and  rear  tires  are  modeled 
by  linear  springs  with  the  same  stiffness  coefficient  Kj.  M^.f  and  M^r  represent  the  masses  of 
unsprung  elements  such  as  the  front  and  rear  wheels  and  their  axles.  Kf,Cf,Kr  and  Cr  are  the 
characteristics  of  the  linear  springs  and  shock  absorbers  that  model  the  suspension  system.  Wa 
and  Wb  constitute  the  wheel  base  and  consequently  specify  the  position  of  the  center  of  gravity  of 
the  sprung  element  (or  vehicle  body). 

center  of  gravity  Oy 


Figure  1:  The  two- wheel  vehicle  model  [2] 


Assuming  that  each  tire  contacts  the  terrain  at  a  point  at  all  times,  four  degrees  of  freedom  exist 
in  the  model:  the  displacements  of  the  unsprung  elements  {xi,X2},  the  bouncing  displacement  of 
the  sprung  element  x^-,  and  the  pitch  angle  9.  All  of  them  are  measured  from  the  corresponding 
static  equilibrium  points. 

In  the  following,  a  coordinate  transformation  between  two  moving  reference  systems  is  described 
first.  The  equations  of  motion  are  subsequently  derived. 
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2.1  Coordinate  Transformation 


To  describe  the  orientation  of  the  vehicle  at  any  time,  the  coordinate  systems  shown  in  Figure  2  are 
defined.  An  inertial  coordinate  system  I  is  chosen  to  be  fixed  on  the  ground.  Another  coordinate 
system  V  moves  with  the  vehicle  with  its  origin  locating  at  the  vehicle’s  center  of  gravity  and  its 
eixes  coinciding  with  the  principal  axes  of  the  vehicle  body.  For  the  two-wheel  vehicle  model,  since 
only  the  longitudinal  axis  changes  with  time,  the  orientation  of  the  vehicle  is  known  if  the  pitch 
angle  is  available.  In  other  words,  if  we  define  the  coordinate  system  I'  as  the  reference  system 

which  translates  with  the  vehicle  but  has  the  same  orientation  as  reference  I,  then  for  any  point 

P,  its  coordinates  in  V  (denoted  by  Pv)  are  related  to  its  coordinates  P/'  in  I'  as  follows: 

Pv  =  RPr  (1) 

where  R  is  the  rotation  matrix  and  is  related  to  the  pitch  angle  by 

^  cos  ^  0  —  sin  0  '' 

P  =  0  1  0  (2) 

I  sin  0  0  cos  6  , 


Y 

Figure  2:  The  coordinate  systems:  The  inertial  coordinate  system  IxYZ-,  the  moving  coordinate 
system  I’x'y'Z'  vehicle  coordinate  system  Vxyz- 
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2.2  Suspension  Dynamics 


To  describe  the  oscillation  behavior  of  the  vehicle  model,  either  the  Newtonian  laws  [9]  or  Lagrange’s 
equations  of  motion  [3]  can  be  apphed.  Because  of  the  simplicity  of  the  two-wheel  model,  it  is  easy 
to  employ  the  Newtonian  laws.  However,  for  a  more  comphcated  model  such  as  a  four-wheel  vehicle 
model  [5,  10],  the  Lagrange  point  of  view  is  more  feasible.  The  equations  of  motion  are  therefore 
derived  in  terms  of  Lagrangian  mechanics  in  this  paper. 

Since  there  exist  four  degrees  of  freedom  in  the  model,  define  the  generalized  coordinates  as 

9  —  ^2!  (3) 

The  Lagrange  equations  of  motion  can  then  be  written  as  follows  [3]: 

±(dT\  ^  ._ 

dt  \dqj  j  dqj  dqj  dqj  ^  ^ 

where  {qj,j=  1, . .  .,4}  constitute  the  generalized  coordinates,  T  is  the  kinetic  energy  of  the  system, 
U  is  the  potential  energy,  D  is  the  dissipation  function,  and  the  Qj's  represent  the  generalized  forces 
acting  on  the  system  during  vibration.  We  describe  each  energy  term  in  the  following. 

A.  Kinetic  Energy 

It  is  well  known  that  the  kinetic  energy  of  a  moving  rigid  body  is  equal  to  the  summation  of 
translational  kinetic  energy  and  rotational  kinetic  energy.  In  other  words,  if  a  rigid  body  moves 
with  translational  velocity  V_  and  angular  velocity  w,  its  kinetic  energy  is  [9] 

-M|yp  -f  -I-  IyyU:"l  -I-  Izz^l)  (5) 

where  M  is  the  mass  of  the  rigid  body,  {Ixx-,Iyy,Izz]  are  the  moments  of  inertia  with  respect  to 
the  principal  axes,  and  are  the  projections  of  the  angular  velocity  along  the  principal 

axes. 

As  seen  from  Figure  2,  since  the  two-wheel  vehicle  model  only  accounts  for  the  bouncing  and 
pitching  motion,  its  rotation  axis  coincides  with  the  pitch  axis  at  aU  times.  The  angular  velocity  is 
therefore  decomposed  into 

=  0 

bJy  =  6  (6) 

=  0 
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Tken,  taking  into  account  the  vertical  displacements  of  the  unsprung  elements  and  the  oscillation 
of  the  sprung  element,  the  kinetic  energy  can  be  obtained  as 


T  =  Tb  +  T^ 


(7) 


where  Tb  is  the  kinetic  energy  contributed  from  the  sprung  element 

TB  =  ^MBiU^^J"-  (8) 

Here  Mb  denotes  the  mass  of  the  sprung  element  and  is  the  kinetic  energy  contributed  from 
unsprung  elements 

(9) 

B.  Potential  Energy 

The  potential  energy  takes  into  account  the  influence  of  the  springs  and  tires  on  the  vehicle  during 
vibration.  Consequently,  to  obtain  the  potential  energy,  the  deformations  of  both  elements  need  to 
be  known.  For  the  tires,  the  deformations  are  directly  observed  from  Figure  1  and  are  obtained  as 


—  X\  ^01 

ds  =  X2-  X02 


(10) 


On  the  other  hand,  noting  that  the  two  corners  to  which  springs  and  shock  absorbers  are 
attached  remain  fixed  in  the  coordinate  system  V,  the  deformations  of  the  springs  are  obtained,  by 
considering  the  displacement  of  center  of  gravity  and  (1),  as 

d2  =  Xc  +  MOi  sin  0  -  xi 
^4  =  Xc  —  Wb  sin  9  —  X2 

Then,  assuming  linear  relationships  hold  between  the  deformations  and  the  forces  exerted  by 
the  springs,  the  potential  energy  can  be  written  as 


U  =  Us +  Ut 


(12) 


where  Us  contains  the  potential  energy  stored  in  the  suspension  systems 

Us  =  ^Kfdl  +  \Krdl 


and  Ut  represents  the  potential  energy  stored  in  the  tires 

Ut  =  \KTd\  +  ^Krdl 


(13) 


(14) 
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C.  Dissipation  Function 

The  dissipation  function  accounts  for  the  effect  of  the  shock  absorbers.  Assuming  that  the  forces 
produced  by  dampers  vary  linearly  with  the  rates  of  change  of  the  deformations,  the  dissipation 
function  D  is  obtained  as 

D  =  \Cfdl  +  \Crdl  (15) 

In  addition  to  the  various  forms  of  energy  described  above,  the  generalized  force  Qj  in  (4),  which 
takes  into  account  other  factors  such  as  the  road  reactions  on  the  tires  due  to  tire  load  variations, 
is  required  to  describe  the  jerky  movements  of  the  vehicle.  The  effects  of  generalized  forces  wiU  be 
considered  in  subsequent  sections.  Then,  by  applying  (4),  the  equations  of  motion  can  be  obtained. 

3  Recursive  Formulation 

It  is  observed  that  the  resulting  equations  of  motion  are  nonlinear.  For  a  vehicle  traveling  through 
uneven  terrain,  it  is  reasonable  to  expect  that  the  pitch  angle  is  small.  Therefore,  in  deriving  the 
equations  of  motion,  if  we  further  assume  that  the  small  pitch  angle  approximation  holds,  i.e. 

sin  ^  ^  (16) 

then  the  equations  of  motion  can  be  described  by  the  foUowing  second  order,  linear  differential 
equation: 

Kq-^  Lq  =  Mg^->r  NQ_  (17) 

where  the  elements  of  the  matrices  K,L,M  and  N  are  related  to  system  parameters  such  as  spring 
constants,  moments  of  inertia,  etc.  Xq  are  the  excitation  inputs  and  Q  are  the  generalized  forces 
defined  as 


(xqi,  X02) 

(18) 

(19) 

If  the  terrain  profile  and  the  generalized  forces  are  known,  the  bouncing  and  pitching  movements 
can  be  obtained  through  the  integration  of  (17).  However,  since  the  system  parameters  are  usually 
known  approximately,  the  results  obtained  from  direct  integration  are  not  reliable.  It  is  therefore 
feasible  to  employ  some  estimation  technique  and  combine  the  information  provided  by  other  sensors 
such  as  cameras  and  accelerometers. 
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Unlike  batch  estimators,  which  process  all  the  data,  recursive  filters  update  the  estimates  of 
parameters  as  new  information  becomes  available.  Among  recursive  estimators,  EKF  and  lEKF 
are  both  suitable  for  estimating  parameters  of  a  nonlinear  system.  We  describe  the  plant  and 
measurement  equations  for  the  recursive  algorithm  in  the  following. 


3.1  The  Plant  Equation 

In  addition  to  the  imperfect  knowledge  of  system  parameters,  it  is  usually  difficult  to  measure  the 
terrain  profile  and  the  generalized  forces  accurately.  To  account  for  these  difficulties,  accelerometers 
are  assumed  to  be  at  our  disposal  to  provide  the  accelerations  of  unsprung  elements.  By  integration 
of  measured  accelerations,  the  velocities  and  displacements  of  unsprung  elements  are  approximately 
derived. 

Therefore,  the  bouncing  displacement  and  the  pitch  angle  are  the  remaining  unknown  quantities. 
Let  the  state  vector  x  be  defined  as  follows: 

x  =  {Xc.,Xc,0,0,v)'^  (20) 

Then  under  the  assumption  that  the  vehicle  moves  along  a  straight  path  with  constant  forward 
speed  V,  the  equations  of  motion  can  be  rewritten  in  the  following  form: 

X  =  Ax  +  Bx^  +  w  (21) 


where  x^  are  regarded  as  control  inputs  consisting  of  displacements  and  velocities  of  unsprung 
elements,  i.e. 

=  (xi,ii,X2,X2)^  (22) 


A,  B  are  the  plant  and  input  matrices  derivable  from  K  and  L  and  have  the  following  form: 


A  = 


0 

-(Kf+Kr) 

Mb 

0 


0 


0 


-(Cf+Cr) 

Mb 

0 


-(KfW^-KrWB)  -{CfWA-CrWB) 
Mb  Mb 


0 


-(KfWA-KrWB)  -jCfWA-CrWB)  -(K  fWl+K'rWl) 


hy 


^yy 

0 


^yy 

0 


-(CfW^,+CrWl) 

lyy 

0 


0 

0 

0 

0 

0 


(23) 
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and 


B 


\ 


0 

0 

0 

0 

K, 

Cf 

Kr 

Cr 

Mb 

Mb 

Mb 

Mb 

0 

0 

0 

0 

KfW^ 

CfW^ 

-KrWB 

-CrW, 

lyy 

lyy 

lyy 

hy 

0 

0 

0 

0 

(24) 


y 


w  is  the  plant  noise,  with  zero  mean  and  covariance  matrix  Qw  The  added  plant  noise  takes  into 
consideration  the  modeling  error  and  the  imprecise  knowledge  of  vehicle  parameters  as  well  as  the 
movements  of  unsprung  elements. 


3.2  The  Measurement  Equations 

A  sequence  of  images  taken  by  a  camera  rigidly  mounted  on  the  moving  vehicle  provides  visual 
information  for  estimating  jerky  movements.  In  order  to  exploit  this  information,  the  relationships 
between  the  state  vector  defined  above  and  the  image  plane  coordinates  of  a  set  of  landmarks  need 
to  be  derived. 

For  clarity,  in  addition  to  the  coordinate  systems  defined  in  Section  2.1,  define  another  reference 
system  as  a  camera  coordinate  system  which  has  a  fixed  orientation  with  respect  to  the  moving 
reference  V  and  its  origin,  Oc^  located  at  the  projection  center  of  the  camera.  In  other  words, 

Ocr(0  =  («fo,0,0  Vt  (25) 

where  both  do  and  /  are  constants.  Moreover,  for  a  vehicle  undergoing  bouncing  and  pitching 
motion  as  well  as  constant  longitudinal  motion,  its  center  of  gravity,  Oy,  follows  the  following 
trajectory  expressed  in  the  inertial  coordinate  system  I: 


Ovi{t)  =  {xc{t) h,Q,vt)  Mt 


(26) 


where  h  is  the  height  of  center  of  gravity  when  the  vehicle  is  at  rest. 

Then,  for  a  landmark  P,  the  relationships  between  its  inertial  coordinates,  Pj  =  (Xojlb,  ■^o)^» 
and  the  camera  centered  coordinates,  Pc(t),  can  be  found  as  follows: 


(  X  \ 

^c(i)  P  ^ 

f 

Pc(t)  =  RiO) 

lo 

- 

0 

- 

0 

V  / 

1  J 

(27) 
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where  R{9)  is  the  rotation  matrix  defined  in  (2).  Without  loss  of  generality,  it  has  been  assumed 
that  the  camera  coordinate  system  has  the  same  pose  as  the  vehicle  reference  system  V. 

After  the  camera  centered  coordinates  have  been  obtained,  the  image  plane  coordinates  of 
landmarks  are  obtained  by  applying  the  perspective  projection  formula.  The  resulting  measurement 
equations  for  the  landmark  are 

,cos  0(ti)[A'oj  -  +  h)]  -  sin^(4-)[Zoj  -  vtj]  -  dp 


y.ffA  —  f - v-cy-.y  .  -VJ  . ---J _ LX  +  7  =  1  N 

^  sme{ti)[XQj-{x,{ti)  +  h)]  +  cose{ti)[ZQj-vti]-l  ^ 


"  ^sme{ti)[Xoj  -  {Xc{ti)  +  h)]  +  cose{ti)[Zoj  -  vti]  -  I  + 


(28) 


where  /  is  the  focal  length  and  N  is  the  number  of  tracked  landmarks.  (nxj{ti),nYj{ti))  is  the 
measurement  noise  which  takes  into  account  the  quantization  noise  and  errors  in  tracking  feature 
points  over  the  sequence. 


4  Simulation  Results 

Using  the  nominal  values  of  the  vehicle  parameters  listed  in  Table  1,  the  performance  of  the  al¬ 
gorithm  in  estimating  the  unstabilized  components  of  the  vehicle’s  motion  was  studied  through 
simulations.  Since  the  movements  of  unsprung  elements  due  to  uneven  terrain  and  generalized 
forces  are  approximately  known  because  of  the  employment  of  accelerometers,  the  effects  of  gen¬ 
eralized  forces  need  not  be  considered  separately.  The  generalized  forces  are  therefore  assumed 
to  be  zero  in  the  simulations.  Then,  after  the  terrain  profile  is  generated,  state  trajectories  ob¬ 
tained  through  the  integration  of  (17)  are  regarded  as  the  ground  truth  for  evaluating  the  filter’s 
performance. 


Table  1:  Model  parameters 


Mwf 

57.5  kg 

Kf 

18.0  kN-m"^ 

Wa 

1.353  m 

^wr 

75.0  kg 

Cs 

1.0  kN  •  m  •  s~^ 

Wb 

1.337  m 

Mb 

1710.0  kg 

Kr 

10.0  kN-m"^ 

h 

2.0  m 

^yy 

1031.25  kg-m^ 

Cr 

1.0  kN  •  m  •  s“^ 

do 

0.6  m 

Kt 

200.0  kN  •  m"^ 

1 

1.35  m 

In  the  following,  the  simulation  results  for  different  terrain  profiles  are  presented. 
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4.1  Deterministic  Excitation 


Consider  the  behavior  of  the  model  when  the  vehicle  encounters  a  bump.  The  bump  is  modeled  by 
a  half  sine  wave  with  its  height  and  width  denoted  by  and  b^,  respectively.  The  movement  of  the 
vehicle  is  such  that  it  first  traverses  a  flat  path  before  encountering  the  bump,  and  the  front  tire 
hits  the  bump  at  the  location  zq.  After  the  vehicle  passes  through  the  bump,  its  excitation  inputs 

are  zero.  This  results  in  an  excitation  input  of  the  front  tire  of  the  following  form: 

✓ 

bh  sin[jf-(ut  -  2o)]  ^  <  t  < 

a:oi(t)=<^  .  (29) 

0  elsewhere 


In  our  work,  we  assumed  bh  =  0.1  m,  b^,  =  0.2  m  and  zq  =  1.345  m. 


Since  the  vehicle  is  assumed  to  move  along  a  straight  path,  the  rear  tire  input  is  related  to  the 
front  tire  input  by 

Xo2{t)  =  xoi  (30) 


where  L  is  the  wheel  base.  Because  the  excitation  inputs  to  the  tires  are  zero  initially,  the  vehicle 
is  assumed  to  be  in  a  static  equilibrium  state  with  a  forward  speed  of  1.345  m  •  s“^  The  resulting 
behavior  of  the  model  is  displayed  in  Figure  3. 


Figure  3:  The  movement  of  the  sprung  element  under  the  excitation  of  a  bump:  (a)  The  bouncing 
displacement  and  the  pitch  angle;  (b)  the  bouncing  velocity  and  the  pitching  velocity. 


Subsequently,  a  set  of  landmarks  is  tracked  over  the  sequence.  Since  the  image  displacement 
fields  due  to  bouncing  and  pitching  movements  are  very  similar,  it  is  important  that  the  landmarks 
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being  tracked  carry  enough  information  for  estimation  of  the  unstabilized  components.  For  example, 
tracking  feature  points  which  are  far  away  from  the  camera  provides  more  information  for  estimating 
the  pitching  motion  than  the  bouncing  behavior.  This  can  be  seen  from  (28).  By  dividing  the 
numerator  and  denominator  by  Zoj  -  the  image  plane  coordinates  for  the  landmark  are  the 
same  but  the  measurement  equation  can  be  rewritten  as 


XjiU)  =  /- 

sii 

Uu)  =  /- 


W) 


l 

V  f.] 

dn 

Zoj—l 

Zoj-r^i 

7 

1 

1 

V 

i 

Zoj—l 

Zoj-r^i 

Zoj—l 

1 

V  f .] 

1 

Zoj-l 

Zoj-r^i 

Zoj—l 

+  nxj{ti) 
+  nYj{ti) 


where  (Xj(to),  Yj{to))  are  the  image  plane  coordinates  of  the  feature  point  at  the  beginning  and 
can  be  found  as  follows: 


X,ito)  = 


Uto)  = 


Xqj  hi  do 


Zqj  —  I 


Yoi 


If  Xc{t)  <  Zoj  -  I,  the  image  plane  displacement  due  to  the  bouncing  movement  is  almost  zero 
and  the  information  is  likely  to  be  overridden  by  errors  in  feature  tracking.  Similarly,  the  spatial 
resolution  of  the  images  should  be  high  enough  that  the  image  plane  displacements  due  to  the 
bouncing  movement  can  be  seen  in  the  sequence. 


Accordingly,  the  image  sequence  is  assumed  to  be  acquired  at  a  rate  of  lOHz.  Each  image  is  of 
size  2.0  X  2.0  and  has  resolution  2000  x  2000.  The  first  four  landmarks  listed  in  Table  2  are  tracked 
over  the  sequence  in  the  simulation.  Note  that  in  generating  these  landmarks,  it  is  further  assumed 
that  the  landmarks  close  to  the  camera  are  likely  to  appear  in  the  lower  part  of  the  images  at  the 
beginning.  For  simplicity,  the  landmarks  are  carefully  chosen  so  that  they  remain  in  the  field  of 
view  at  all  times. 


Table  2:  The  3-D  coordinates  of  landmarks  in  the  inertial  coordinate  system  I 


Landmarks 

3-D  coordinates 

Landmarks 

3-D  coordinates 

1 

-13.75 

-15.75 

22.50 

5 

-29.50 

-31.50 

45.00 

2 

-14.80 

-8.40 

21.00 

6 

-31.60 

-16.80 

42.00 

3 

-15.25 

11.50 

23.00 

7 

-32.50 

23.00 

46.00 

4 

-10.90 

17.20 

21.50 

8 

-23.95 

34.60 

43.25 

As  mentioned  above,  the  movements  of  the  unsprung  elements  need  to  be  known  in  order  to 
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apply  the  lEKF.  Since  they  are  obtained  from  the  integration  of  measured  accelerations,  errors  are 
expected.  Uniformly  distributed  errors  with  variances  proportional  to  the  true  values  are  therefore 
added  to  model  uncertainties.  For  comparison,  the  resulting  errors  in  the  movements  of  the  sprung 
element  obtained  from  direct  integration  of  (21)  are  shown  in  Figure  4. 


X  10’^ 


X  10' 


(a)  (b) 

Figure  4:  The  errors  in  the  sprung  element’s  movement  obtained  from  direct  integration  (deter¬ 
ministic  excitation):  (a)  The  bouncing  displacement  and  the  pitch  angle;  (b)  the  bouncing  velocity 
and  the  pitch  velocity. 


After  the  measurements  and  control  inputs  are  obtained,  assuming  the  vehicle  is  in  the  equilib¬ 
rium  state  and  moves  forward  at  a  speed  of  1.0  m  -s”^  initially,  the  bias  and  Root-Mean-Squared 
Errors  (RMSE)  obtained  from  twenty  Monte  Carlo  runs  are  shown  in  Figures  5  and  6.  Since  the 
estimate  of  the  forward  speed  is  quite  accurate,  the  corresponding  result  is  not  shown  here. 

To  compare  our  results  with  other  kinematic-model-based  algorithms,  a  kinematic  model  which 
assumes  that  the  camera  moves  with  constant  translation  and  rotation  is  adopted.  The  plant 
equation  for  the  kinematic  model  is  therefore 

X  =  Ax  -|-  w  (31) 

where  x  is  the  state  vector  defined  in  (20),  and  A  is  the  sparse  square  matrix 

A  =  {Ai2  =  A34  =  1;  all  other  elements  Aij  =  0}  (32) 


w  is  the  plant  noise. 
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Figure  5: 
for  the  ki 
the  boun 


As  seen  from  Figures  5  and  6,  for  xdt)  and  d{t),  both  models  provide  good  yet  similar  estimates. 
This  is  due  to  the  direct  relationships  between  corresponding  states  and  measurements.  As  for  the 
other  two  states,  and  ^(t),  the  kinetic  model  responds  to  the  excitation  faster  than  the 

kinematic  model  because  of  better  modeling.  In  addition,  it  is  observed  that  the  estimates  of  0{t) 
and  0{t)  are  more  accurate  than  the  estimates  of  Xc{t)  and  Xc{t)  since  the  measurements  provide 
diiferent  information  as  mentioned  above. 

Although  the  kinematic  model  provides  estimates  comparable  to  the  kinetic  model  after  the 
transient,  there  are  irregularities  continuously  acting  on  the  tires  in  the  real  world.  We  study  the 
responses  of  both  models  under  continuous  irregularities  in  the  next  section. 

4.2  Stochastic  Excitation 

For  simplicity,  the  irregularity  is  modeled  by  a  first  order  Markov  process  with  coefficient  depending 
on  surface  roughness  and  the  vehicle’s  forward  speed.  Specifically,  the  excitation  input  to  the  front 
tire  is  modeled  as 

xoi  (t)  =  ~av  xoi(t)  +  n{t)  (33) 

where  n{t)  is  zero  mean,  white  Gaussian  noise  with  variance  equal  to  ‘la^av.  The  values  of  the 
coefficients,  a  and  cr,  vary  with  the  roughness  of  surface,  v  is  the  vehicle’s  forward  speed.  In 
particular,  a  =  0.012  m,  a  =  0.40  m"^  and  v  =  13.45  m  •  s~^  were  chosen  in  our  work  [8].  Figure  7 
shows  a  sample  function  of  the  random  process.  Then,  according  to  (30),  the  excitation  of  the  rear 
tire  is  also  specified.  The  resulting  movement  of  the  sprung  element  is  shown  in  Figure  8. 


Stochastic  Exciution  of  the  Front  Tyre 


Figure  7:  The  stochastic  excitation  to  the  front  tire. 
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(a)  (b) 

Figure  8:  The  movement  of  the  sprung  element  under  stochastic  excitation;  (a)  The  bouncing 
displacement  and  the  pitch  angle;  (b)  the  bouncing  velocity  and  the  pitch  velocity. 


Since  the  speed  of  the  vehicle  is  higher,  a  higher  image  acquisition  rate  is  assumed  in  this  case. 
Accordingly,  the  last  four  landmarks  listed  in  Table  2  are  tracked  and  an  image  sequence  is  acquired 
at  the  rate  of  20Hz  in  which  each  image  has  the  same  size  and  resolution  as  before.  The  choice  of 
the  last  four  landmarks  is  such  that  they  remain  in  the  field  of  view  for  more  than  three  seconds. 
In  addition,  because  the  four  landmarks  are  almost  out  of  the  field  of  view  in  three  seconds,  only 
fifty  frames  are  considered. 

As'  in  the  previous  case,  the  movements  of  the  unsprung  elements  are  assumed  to  be  noisy. 
Moreover,  in  estimating  the  motion  of  the  sprung  element,  unlike  the  previous  case,  the  vehicle  is 
not  in  the  equilibrium  state  when  the  processing  begins.  Instead,  we  assume  the  initially  its  center 
of  gravity  is  0.01  m  higher  than  the  equilibrium  point,  the  bouncing  velocity  is  equal  to  0.1  m  •  s“^, 
the  pitch  angle  is  0.01  rad,  the  pitch  velocity  is  0.1  rad  -s”^,  and  the  forward  speed  is  10.0  m  •  s~^. 
Then,  with  the  initial  conditions  different  from  the  exact  starting  states,  the  effects  of  imperfect 
knowledge  are  shown  in  Figure  9;  the  estimated  results  of  both  kinetic  and  kinematic-model  based 
algorithms  from  twenty  Monte  Carlo  runs  are  displayed  in  Figures  10  and  11. 

As  seen  from  the  figures,  the  lEKF  reaches  a  steady  state  quite  quickly  and  improves  its 
performance  over  direct  integration  by  incorporating  visual  information.  Furthermore,  the  kinetic 
model  outperforms  the  kinematic  model  because  of  its  better  response  to  continuous  irregularities. 

Now  that  the  performance  of  the  filter  with  exact  knowledge  of  a  set  of  internal  parameters  has 
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(a) 


(b) 


Figure  9:  The  errors  in  the  sprung  element’s  movement  estimated  from  direct  integration  (stochastic 
excitation):  (a)  Bouncing  displacement  and  pitch  angle;  (b)  bouncing  velocity  and  pitch  velocity. 


been  studied,  the  robustness  issue  needs  to  be  addressed.  In  the  next  section  we  investigate  the 
sensitivity  of  the  filter  to  mismatches  of  various  parameters. 


5  Sensitivity  Analysis 

The  proposed  algorithm  exploits  knowledge  not  used  by  other  kinematic-model-based  algorithms 
to  yield  enhanced  performance.  However,  to  estimate  the  bouncing  and  pitching  movements,  a  set 
of  vehicle  parameters  needs  to  be  known.  In  addition  to  the  problem  of  measuring  them  precisely, 
some  parameters  are  likely  to  vary  during  the  operation  of  the  vehicle,  e.g.  spring  stiffness.  It  is 
therefore  necessary  to  address  the  robustness  of  the  filter. 

In  our  formulation,  since  the  wheel  movements  x^{t)  are  approximately  known,  the  effect  of  tire 
stiffness  mismatch  is  not  considered.  The  remaining  parameters  which  affect  the  behavior  of  the 
model,  i.e.  the  A  and  B  matrices  defined  in  (23),  (24),  are  characteristic  constants  of  the  springs 
and  dampers  which  consist  of  the  suspension  system,  the  vehicle  mass  Mb  and  moment  of  inertia 
lyy,  and  the  position  of  the  center  of  gravity  Wa-  Note  that  because  the  sum  of  Wa  and  Wb  is 
fixed,  it  is  sufficient  to  study  the  effect  of  Wa  only.  In  the  following  we  present  both  analytic  and 
numerical  analysis  results  subject  to  the  excitation  input  shown  in  Figure  7. 
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Figure  10:  Bias  in  the  estimates  of  the  sprung  element’s  movement  under  stochastic  extitation  for 
the  kinetic  (solid  hne)  and  kinematic  (dashed  line)  models:  (a)  The  bouncing  displacement;  (b)  the 
bouncing  velocity;  (c)  the  pitch  angle;  (d)  the  pitch  velocity. 


5.1  Analytic  Sensitivity  Analysis 


The  lEKF  estimates  the  unstabilized  components  by  combining  the  information  in  the  dynamic 
model  and  visual  data.  As  shown  in  the  comparison  of  the  kinetic  and  kinematic  models,  since  not 
aU  of  the  states  are  directly  related  to  the  image  plane  movements  of  landmarks,  it  is  important 
that  the  dynamic  model  provide  a  good  description  of  these  states.  The  sensitivity  of  the  kinetic 
model  to  various  parameters  is  therefore  investigated. 

For  convenience,  define  the  parameter  vector  A  as 

A  -  [A'y,  AV,  Cf,  Cr,  Wa,  Mb,  lyyf  (34) 

and  denote  its  nominal  values  by  Aq.  Then  assuming  the  plant  noise  in  (21)  is  zero,  the  dynamic 
model  is  described  by 


=  /[A,1(<,A),£u;(^)] 


(35) 

(36) 


The  effect  of  parameter  mismatches  on  x{t,X)  is  studied  using  perturbation  theory  [6].  We  focus 
on  estimating  the  first  order  effect  of  parameter  perturbations,  say  the  sensitivity  function.  If  we 
assume  that  /[A,^(^  A)5^u,(0]  continuously  differentiable  with  respect  to  x  and  A  and  define  the 
sensitivity  function  S(t)  as 


5(0  = 


dx{t,  A) 

dx 


lA=Ao 


(37) 


it  can  be  shown  [6]  that  when  ||A  —  Ad  II  is  sufficiently  small,  S{t)  satisfies  the  following  differential 
equation: 


5(0  =  F(Ao)5(0  +  G(Ao,x,.),  S{to)  =  0 


(38) 


where 


nAo) 

^■(Ao;  ^lii) 


dx 

dx 


x=x{t,^) 

r=r(t,Ao) 


(39) 

(40) 


and  ^(t,  Aq)  is  the  solution  of  (35)  with  the  nominal  values  of  the  parameter  vector. 

Noting  that  Xy^{t)  is  available,  5(0  can  be  easily  obtained  through  numerical  integration.  Fig¬ 
ure  12  shows  the  resulting  5(0-  It  has  been  observed  that  aU  the  elements  of  x(t,Ao)  are  much 
more  sensitive  to  Wa  than  the  other  parameters.  In  order  to  see  the  effects  of  mismatches  in  the 
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other  parameters,  the  sensitivities  of  z(t,A)  with  respect  to  Wa  are  shown  in  (a)  and  (b)  separately. 
Figure  12  (c)  to  (f)  show  the  sensitivities  of  each  state  to  the  other  parameters.  Since  most  of  them 
are  very  small,  we  only  show  the  effects  of  the  three  most  sensitive  parameters. 

As  seen  from  Figure  12,  in  addition  to  the  high  sensitivity  to  Wa,  the  bouncing  movements  are 
more  sensitive  to  mismatches  in  Mg',  the  pitching  behavior  is  more  sensitive  to  mismatches  in  lyy. 
Moreover,  Cr  has  moderate  effects  on  all  the  states,  and  the  model  seems  to  be  less  sensitive  to 
spring  stiffness  mismatches. 

Under  the  assumption  that  mismatches  in  the  parameters  are  sufficiently  small,  the  sensitivity 
function  gives  us  a  qualitative  idea  of  how  sensitive  the  model  is  to  parameter  mismatches.  How¬ 
ever,  depending  on  the  nominal  values,  mismatches  can  be  moderate  for  some  parameters,  and  the 
filter  incorporates  additional  visual  information  to  improve  the  estimates  of  both  unstabilized  com¬ 
ponents.  The  sensitivity  of  the  filter  to  parameter  mismatches  is  therefore  studied  quantitatively 
in  the  next  section. 

5.2  Monte-Carlo  Simulations 

Because  of  the  effect  of  plant  and  measurement  noise  as  well  as  imperfect  knowledge  of  the  unsprung 
elements’  movements,  sensitivity  to  parameter  mismatches  is  studied  through  Monte  Carlo  methods. 
We  focus  here  on  the  analysis  for  the  stochastic  excitation  case.  Therefore,  using  the  parameters 
listed  in  Table  1  as  nominal  values,  the  ground  truth  for  the  states  and  measurement  trajectories 
is  generated  as  in  Section  4.2.  Then,  by  varying  each  parameter  separately  within  a  reasonable 
range,  the  performance  of  the  filter  is  investigated. 

In  the  following  study,  we  first  classify  the  parameters  into  three  categories:  suspension  pa¬ 
rameters,  inertial  parameters,  and  basic  dimensions.  Then,  assuming  the  initial  conditions  of  the 
lEKF  are  the  same  as  those  in  the  study  of  stochastic  excitation,  we  present  the  effects  of  param¬ 
eter  mismatches  in  each  category.  For  simplicity,  since  the  bias  in  the  estimates  is  small  after  the 
filter  reaches  the  steady  state,  the  average  RMSE  over  the  last  25  frames,  i.e.  t  e  [0.125,0.25],  is 
computed  for  comparison.  With  the  nominal  values  listed  in  Table  1,  the  average  RMS  error  in 
the  estimate  of  Xc{t)  is  0.005911,  in  Xc{t)  it  is  0.021331,  in  e{t)  it  is  0.000157,  in  e{t)  it  is  0.006673, 
and  in  v  it  is  0.002994. 

(1)  Suspension  Parameters:  The  suspension  parameters  consist  of  Kf,Kr,Cf  and  Cr-  For  each 
component,  two  values  corresponding  to  ±50%  mismatches  of  the  nominal  value  are  consid- 
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Figure  12:  Sensitivity  functions  under  random  excitation. 
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ered.  The  resulting  effects  are  shown  in  Table  3.  Similar  to  the  results  shown  in  Section  4.2, 
a;c(/)  and  9{t)  are  insensitive  to  suspension  parameter  mismatches.  As  for  Xc{t)  and  6{t), 
although  the  parameter  mismatch  is  up  to  50%,  the  performance  of  the  filter  degrades  only 
a  little. 


Table  3:  The  effects  of  suspension  parameters 


Parameters 

RMSE  of  Xc{t) 

RMSE  of  Xc{t) 

RMSE  of  e{t) 

RMSE  of  e(t) 

RMSE  of  V 

Kf 

9000.0  (-50%) 

27000.0  (-1-50%) 

0.005747 

0.005762 

0.019784 

0.023697 

0.000138 

0.000134 

0.007020 

0.011747 

0.002500 

0.002417 

Kr 

5000.0  (-50%) 

15000.0  (-1-50%) 

0.005845 

0.006084 

0.020547 

0.023672 

0.000142 

0.000164 

0.011751 

0.007695 

0.002554 

0.002916 

Cf 

500.0  (-50%) 

1500.0  (-1-50%) 

0.005833 

0.006040 

0.021439 

0.021602 

0.000140 

0.000167 

0.007078 

0.006849 

0.002498 

0.003027 

Cr 

500.0  (-50%) 

1500.0  (-1-50%) 

0.005453 

0.005800 

0.021577 

0.020544 

0.000132 

0.000149 

0.006453 

0.008852 

0.002323 

0.002772 

(2)  Inertial  Parameters:  We  consider  the  mismatches  of  Mb  and  lyy.  Since  these  two  parameters 
are  directly  related  to  the  sprung  element’s  movements,  the  performance  of  the  filter  is  ex¬ 
pected  to  be  more  sensitive  to  parameter  mismatches.  By  varying  each  parameter  separately 
by  -50%,  -25%,  25%  and  50%  of  the  corresponding  nominal  value,  the  average  RMSEs 
resulting  from  the  lEKF  are  shown  in  Table  4.  As  seen  in  Table  4,  when  Mb  or  lyy  is  only 
half  of  the  nominal  value,  the  average  error  increases  rapidly  compared  to  the  other  cases. 
The  model  also  seems  to  be  more  sensitive  to  smaller  values  of  Mb  and  lyy.  Moreover,  the 
mismatch  of  Mb  only  affects  the  estimates  of  Xc{t)  and  Xc{t),  but  not  those  of  9{t)  and  6{t). 
The  opposite  is  true  for  the  effect  of  lyy. 


Table  4:  The  effects  of  inertial  parameters 


Parameters 

RMSE  of  Xc{t) 

RMSE  of  ic(t) 

RMSE  of  9{t) 

RMSE  of^(t) 

RMSE  of  V 

855.0 

(-50%) 

0.005453 

0.031434 

0.000140 

0.006467 

0.002593 

Mb 

1282.5 

(-25%) 

0.005663 

0.023491 

0.000147 

0.006285 

0.002681 

2137.5 

(-1-25%) 

0.005749 

0.021482 

0.000142 

0.006402 

0.002453 

2565.0 

(-1-50%) 

0.006085 

0.022240 

0.000166 

0.006645 

0.003005 

515.6 

(-50%) 

0.005594 

0.021763 

0.000137 

0.019551 

0.002512 

773.4 

(-25%) 

0.006056 

0.022475 

0.000169 

0.010187 

0.003154 

1289.1 

(-1-25%) 

0.005902 

0.021990 

0.000151 

0.006448 

0.002811 

1546.9 

(-1-50%) 

0.005907 

0.022386 

0.000142 

0.007306 

0.002533 

(3)  Basic  Dimensions:  After  the  sensitivity  to  mismatches  of  the  suspension  and  inertial  param¬ 
eters  has  been  studied,  Wa  is  the  remaining  parameter  which  affects  the  plant  and  input 
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matrices.  The  resulting  performance  of  the  filter  when  this  parameter  is  mismatched  by 
—50%,  —25%,  25%,  or  50%  of  the  nominal  value  is  shown  in  Table  5.  As  seen  in  Table  5,  the 
filter  is  not  very  sensitive  to  mismatches  in  the  parameter  within  25%,  but  if  the  mismatch 
is  too  large,  the  error  in  the  estimate  of  0  starts  to  increase. 


Table  5:  The  effects  of  basic  dimensions 


Parameters 

RMSE  of  Xc(t) 

RMSE  of  ic(<) 

RMSE  of  ff(t) 

RMSE  of  0(i) 

RMSE  of  V 

0.676 

(-50%) 

0.005627 

0.020987 

0.000143 

0.012693 

0.002629 

Wa 

1.015 

(-25%) 

0.005708 

0.022588 

0.000151 

0.007024 

0.002813 

1.691 

(4-25%) 

0.005607 

0.022920 

0.000131 

0.009199 

0.002191 

2.030 

(-f50%) 

0.005624 

0.025048 

0.000149 

0.012803 

0.002917 

Through  our  quantitative  analysis,  it  is  observed  that  the  incorporation  of  visual  information 
provides  robust  estimates  of  Xc{t)  and  0(t).  And  if  the  mismatches  in  various  parameters  are 
moderate,  say  less  than  25%  of  the  nominal  values,  the  performance  of  the  filter  does  not  degrade 
much.  Therefore,  we  claim  that  the  filter  is  robust  with  respect  to  parameter  mismatches. 

6  Conclusions 

A  new  kinetic-model-based  algorithm  has  been  presented  for  estimating  some  unstabilized  com¬ 
ponents  of  vehicular  motion.  With  the  incorporation  of  existing  kinematic-model-based  motion 
estimation  algorithms,  a  better  description  of  the  vehicular  motion  can  be  obtained.  Consequently, 
many  navigation-related  tasks  can  be  simplified.  Although  the  3-D  landmarks  are  assumed  to  be 
known  in  this  paper,  structure  from  motion  algorithms  could  be  incorporated  so  that  structure  and 
motion  could  be  estimated  simultaneously.  Because  of  improved  modeling  and  the  use  of  inertial 
sensors,  better  performance  is  possible. 

In  addition  to  its  application  to  image  understanding,  the  consideration  of  unstabilized  com¬ 
ponents  of  vehicle  motion  provides  information  for  improving  the  ride  behavior  of  the  vehicle.  In 
order  to  compensate  for  the  external  disturbances,  active  suspension  systems  (which  differ  from 
passive  systems  in  using  the  actuators  to  continuously  generate  forces  acting  on  the  wheels  and 
vehicle  body)  have  been  suggested.  Estimates  of  the  unstabilized  components  can  provide  useful 
information  in  the  design  of  active  suspension  systems. 
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